package roborl.wheelRobo;


import java.util.Vector;

import roborl.Action;
import roborl.Decision;
import roborl.DiscreteValueApr;
import roborl.PhysRobot;
import roborl.PhysicsWorld;
import roborl.RobotState;

import net.phys2d.math.Vector2f;
import net.phys2d.raw.ActuatorJoint;
import net.phys2d.raw.Body;
import net.phys2d.raw.Joint;
import net.phys2d.raw.shapes.Box;
import net.phys2d.raw.shapes.Circle;

public class WheelRobo extends PhysRobot {
	// public static final Action rot_none = new Action("ROT_NONE");
	public static final Action rot_cw = new Action("ROT_CW");
	public static final Action rot_ccw = new Action("ROT_CCW");
	public static final Vector<Action> as;
	static {
		as = new Vector<Action>();
		// as.add(rot_none);
		as.add(rot_cw);
		as.add(rot_ccw);
	}

	public WheelRobo() {
		super.DECISION_ITERATION = 50;
		super.SILENT_ITERATION = 0;
		DiscreteValueApr apr = new DiscreteValueApr(WheelRobo.as);
		Decision dec = new Decision(apr, new SimpleRoboReward());
		setDecision(dec);
		// TODO Auto-generated constructor stub
		Vector<Object> parts = getParts();

		float x = 250;
		float y = 100;
		Body body1 = new Body(new Box(100, 25), 1);
		body1.setPosition(x, y);
		parts.add(body1);

		Body wheel1 = new Body(new Circle(20), 1);
		wheel1.setPosition(x - 60f, y + 35);
		parts.add(wheel1);

		Body wheel2 = new Body(new Circle(20), 1);
		wheel2.setPosition(x + 60f, y + 35);
		parts.add(wheel2);

		Joint j1 = new ActuatorJoint(body1, wheel1, new Vector2f(-60, 35),
				new Vector2f());
		Joint j2 = new ActuatorJoint(body1, wheel2, new Vector2f(60, 35),
				new Vector2f());

		parts.add(j1);
		parts.add(j2);

		getAcutator().add(wheel1);
		getAcutator().add(wheel2);
	}

	@Override
	public RobotState getState() {
		// TODO Auto-generated method stub
		// return sensor.getState(this);
		RobotState robotState = new RobotState() {
			double d, s;
			{
				Body body = (Body) getParts().get(0);
				d = (body.getPosition().getX() > 500 ? -1 : 1);
				s = body.getVelocity().getX() > 0 ? 1 : -1;
			}

			@Override
			public Double getState(String meta) {
				// TODO Auto-generated method stub
				if (meta.equalsIgnoreCase("location")) {
					return d;
				} else if (meta.equalsIgnoreCase("speed")) {
					return s;
				}
				return null;
			}

			@Override
			public Vector<String> getStateMeta() {
				// TODO Auto-generated method stub
				Vector<String> vector = new Vector<String>();
				vector.add("location");
				vector.add("speed");
				return vector;
			}

			@Override
			public Vector<Double> getStates() {
				// TODO Auto-generated method stub
				Vector<Double> sv = new Vector<Double>();
				Body body = (Body) getParts().get(0);
				sv.add(d);
				sv.add(s);
				return sv;
			}

			@Override
			public Vector<Double> getHiddenStates() {
				// TODO Auto-generated method stub
				return null;
			}

		};
		return robotState;
	}

	@Override
	public void takeAction(Action action) {
		// TODO Auto-generated method stub

	}

	@Override
	protected void simpleUpdate() {
		// TODO Auto-generated method stub
		if (lastAction != null) {
			/*
			 * if (lastAction == rot_none) { for (Body actuatorJoint : acutator) {
			 * actuatorJoint.setAngularVelocity(0); } } else
			 */if (lastAction == rot_cw) {
				for (Body actuatorJoint : acutator) {
					actuatorJoint.setAngularVelocity(0.5f);
				}
			} else if (lastAction == rot_ccw) {
				for (Body actuatorJoint : acutator) {
					actuatorJoint.setAngularVelocity(-0.5f);
				}
			}
		}
	}

	public static void main(String s[]) throws InterruptedException {
		final PhysicsWorld physicsWorld = new PhysicsWorld();
		PhysRobot physRobot = new WheelRobo();

		new Thread() {
			public void run() {
				physicsWorld.start();
			}
		}.start();

		Thread.sleep(500);
		physicsWorld.addRobot(physRobot);
	}

	@Override
	public Vector<Action> getAvailableActions() {
		// TODO Auto-generated method stub
		return as;
	}
}
